#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <time.h>
// #include "common.h"

#define DYNCLK_SIZE 0xFFFF
#define DYNCLK 0x00A0000000

static const int MMCM[29]=
{
0x000,0x004,0x008,0x00C,
0x010,0x200,0x204,
0x208,0x20C,0x210,//CLKOUT0
0x214,0x218,0x21C,//CLKOUT1
0x220,0x224,0x228,//CLKOUT2
0x22C,0x230,0x234,//CLKOUT3
0x238,0x23C,0x240,//CLKOUT4
0x244,0x248,0x24C,//CLKOUT5
0x250,0x254,0x258,//CLKOUT6
0x25C
};

enum MMCM_Reg
{
Software_Reset,DYNCLK_Status,Clock_Monitor,Interrupt_Status,
Interrupt_Enable,ClockConfig0,CLKFBOUT_PHASE,
CLKOUT0_DIVIDE,CLKOUT0_PHASE,CLKOUT0_DUTY,
CLKOUT1_DIVIDE,CLKOUT1_PHASE,CLKOUT1_DUTY,
CLKOUT2_DIVIDE,CLKOUT2_PHASE,CLKOUT2_DUTY,
CLKOUT3_DIVIDE,CLKOUT3_PHASE,CLKOUT3_DUTY,
CLKOUT4_DIVIDE,CLKOUT4_PHASE,CLKOUT4_DUTY,
CLKOUT5_DIVIDE,CLKOUT5_PHASE,CLKOUT5_DUTY,
CLKOUT6_DIVIDE,CLKOUT6_PHASE,CLKOUT6_DUTY,
LoadConfig
};
//store the value of all the clock configuration registers
static int MMCM_value[29];

struct Reg_Params
{
	float Freq;
	float BOUT_MULT;
	int   DIVCLK_DIVIDE;
	float CLKOUT_DIVIDE;
	unsigned char  BOUT_MULT_I;//BOUT_MULT integer part
	unsigned short BOUT_MULT_F;//BOUT_MULT fractional part
	unsigned char  CLKOUT_DIVIDE_I;//BOUT_MULT integer part
	unsigned short CLKOUT_DIVIDE_F;//BOUT_MULT fractional part
	int ClkConfig0;//
	int CLKOUT;//
};

int DYNCLK_Read_Reg(void* DYNCLK_Addr, int Offset)
{
	int Value;
	Value=*((unsigned*)(DYNCLK_Addr + MMCM[Offset]));
	return Value;
}

int DYNCLK_Write_Reg(void* DYNCLK_Addr, int Offset, int Value)
{
	*((unsigned*)(DYNCLK_Addr + MMCM[Offset]))=Value;
	return 0;
}

struct Reg_Params ClkGetParams(float Freq)
{
	struct Reg_Params tmp;
	if ((Freq >= 20) && (Freq < 500))
	{
		tmp.DIVCLK_DIVIDE = 5;
		tmp.CLKOUT_DIVIDE = 5;
		tmp.BOUT_MULT = Freq / 4.0;
	}
	else
	{
		printf("[DYNCLK] %f is out of range (20MHz to 500MHz).\n", Freq);
		exit(0);
	}

	tmp.BOUT_MULT_I = (int)tmp.BOUT_MULT;
	tmp.BOUT_MULT_F = (int)((tmp.BOUT_MULT - tmp.BOUT_MULT_I) * 1000);
	tmp.CLKOUT_DIVIDE_I = (int)tmp.CLKOUT_DIVIDE;
	tmp.CLKOUT_DIVIDE_F = (int)((tmp.CLKOUT_DIVIDE - tmp.CLKOUT_DIVIDE_I) * 1000);
	tmp.ClkConfig0 = (tmp.BOUT_MULT_F << 16) + (tmp.BOUT_MULT_I<<8) + tmp.DIVCLK_DIVIDE;
	tmp.CLKOUT = (tmp.CLKOUT_DIVIDE_F << 8) + tmp.CLKOUT_DIVIDE_I;

	return tmp;
}

void DYNCLK_Config_Freq(void* DYNCLK_Addr, int DYNCLK_id, float Freq)
{
	int Status;
	struct Reg_Params DYNCLK_Config = ClkGetParams(Freq);
	DYNCLK_Write_Reg(DYNCLK_Addr, ClockConfig0, DYNCLK_Config.ClkConfig0);
	DYNCLK_Write_Reg(DYNCLK_Addr, DYNCLK_id * 3 + 7, DYNCLK_Config.CLKOUT);

	do
	{
		Status=DYNCLK_Read_Reg(DYNCLK_Addr, DYNCLK_Status);
	}
	while(Status != 0x01);

	DYNCLK_Write_Reg(DYNCLK_Addr,LoadConfig, 0x03);
}

float Get_DYNCLK_Freq(void* DYNCLK_Addr, int DYNCLK_id)
{
	int R0 = DYNCLK_Read_Reg(DYNCLK_Addr,ClockConfig0);
	int CLKOUT = DYNCLK_Read_Reg(DYNCLK_Addr,DYNCLK_id*3+7);
	unsigned char R0_DIVIDE = R0 & 0xFF;
	unsigned char R0_MULT_I = (R0 & 0xFF00)>>8;
	unsigned short R0_MULT_F = (R0 & 0x3FF0000)>>16;
	unsigned char CLKOUT_DIVIDE_I = CLKOUT & 0xFF;
	unsigned short CLKOUT_DIVIDE_F = (CLKOUT & 0x3FF00)>>8;
	float CLKFBOUT_MULT = R0_MULT_I + (float)R0_MULT_F/1000;
	float CLKOUT_DIVIDE = CLKOUT_DIVIDE_I + (float)CLKOUT_DIVIDE_F/1000;
	//printf("Clock Configuration Register 0 = 0x%x, DIVCLK_DIVIDE = %d, CLKFBOUT_MULT = %f\n", R0, R0_DIVIDE, CLKFBOUT_MULT);
	//printf("CLKOUT%d_DIVIDE = %f\n", DYNCLK_id, CLKOUT_DIVIDE);
	float Freq = 100 * CLKFBOUT_MULT /(CLKOUT_DIVIDE*R0_DIVIDE);
	return Freq;
}

// int main(int argc, char* argv[])
// {
// 	void* DYNCLK_ptr;

//  	int ID = atoi(argv[1]);
// 	int Freq = atoi(argv[2]);
// 	// open mem
// 	int mem_fd;
// 	mem_fd = open("/dev/mem", O_RDWR);
// 	if (mem_fd == -1)
// 	{
// 		printf("CANNOT open /dev/mem!\n");
// 		return -1;
// 	}
// 	DYNCLK_ptr = mmap(NULL, DYNCLK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, DYNCLK);
// 	DYNCLK_Config_Freq(DYNCLK_ptr, ID, (float)Freq);
// 	float retFreq = Get_DYNCLK_Freq(DYNCLK_ptr, ID);
// 	printf("CLK_out%d is set to %.3fMHz\n", ID, retFreq);
// 	return 0;
// }

float DYNCLK_set_read(int ID, float freq)
{
	// open mem
	int mem_fd = open("/dev/mem", O_RDWR);
	if (mem_fd == -1)
	{
		printf("[DYNCLK_set_read] CANNOT open /dev/mem!\n");
		return 0.0;
	}
	void* DYNCLK_ptr = mmap(NULL, DYNCLK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, DYNCLK);
	if (DYNCLK_ptr == MAP_FAILED)
	{
		printf("[DYNCLK_set_read] CANNOT mmap /dev/mem!\n");
		return 0.0;
	}
	DYNCLK_Config_Freq(DYNCLK_ptr, ID, freq);
	float retFreq = Get_DYNCLK_Freq(DYNCLK_ptr, ID);
	munmap(DYNCLK_ptr, DYNCLK_SIZE);
	printf("[DYNCLK_set_read] CLK_out%d is set to %.3fMHz\n", ID, retFreq);
	return retFreq;
}

float DYNCLK_set_read_silent(int ID, float freq)
{
	// open mem
	int mem_fd = open("/dev/mem", O_RDWR);
	if (mem_fd == -1)
	{
		// printf("[DYNCLK_set_read] CANNOT open /dev/mem!\n");
		return 0.0;
	}
	void* DYNCLK_ptr = mmap(NULL, DYNCLK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, DYNCLK);
	if (DYNCLK_ptr == MAP_FAILED)
	{
		// printf("[DYNCLK_set_read] CANNOT mmap /dev/mem!\n");
		return 0.0;
	}
	DYNCLK_Config_Freq(DYNCLK_ptr, ID, freq);
	float retFreq = Get_DYNCLK_Freq(DYNCLK_ptr, ID);
	munmap(DYNCLK_ptr, DYNCLK_SIZE);
	// printf("[DYNCLK_set_read] CLK_out%d is set to %.3fMHz\n", ID, retFreq);
	return retFreq;
}
